#include "SceneObject.h"
#include "Types.h"
#include "Debug.h"
#include "Helpers.h"
#include "MemoryStream.h"
#include "GameManager.h"

using namespace irr;
using namespace core;
using namespace io;

class CGameManager;

f32 accelConstant = 100;

bool CSceneObject::collisionCallback(CSceneObject *obj) {
	DEBUG_PRINT("Unhandled Collision - ");
	DEBUG_PRINT("ID:%d,Type:%d -> ID:%d,Type:%d\n", this->getID(), this->getType(), 
													obj->getID(), obj->getType());
	DEBUG_PRINTVECT("At: ", obj->getPosition());
	return false;
} 
void CSceneObject::handleEvents(CGameManager *engine) {}
void CSceneObject::passTime( f32 frameDelta) {

}
void CSceneObject::doMovement(f32 frameDelta) {
	vector3df vec;

	if(this->mPhysxObject && this->mSceneNode) {

		if(this->mAcceleration > 0) {
			matrix4 mat;
			vector3df speed = vector3df(0,0,(this->mAcceleration * frameDelta));
			mat.setRotationDegrees(this->getRotation());
			mat.translateVect(speed);
			
			this->mPhysxObject->getActor()->addLocalForce(NxVec3(speed.X, speed.Y, speed.Z), NxForceMode::NX_SMOOTH_VELOCITY_CHANGE);
		}

		this->mPhysxObject->getPosition(vec);
		this->mSceneNode->setPosition(vec);

		this->mPhysxObject->getRotation(vec);
		this->mSceneNode->setRotation(vec);
	}
}

/* Accessors and helpers */
void				CSceneObject::move(irr::core::vector3df vel) {
   matrix4 m;
 
	m.setRotationDegrees(this->getVelocity());
    m.transformVect(vel);

    this->setPosition(this->getPosition() + vel);
}

void				CSceneObject::rotate(vector3df rot)
{
    matrix4 m;
	matrix4 n;

	m.setRotationDegrees(this->getRotation());
    n.setRotationDegrees(rot);
    m *= n;
    this->setRotation( m.getRotationDegrees());
}

void				CSceneObject::turn(f32 rot)
{
    this->rotate(vector3df(0.0f, rot, 0.0f) );
}

void				CSceneObject::pitch(f32 rot)
{
    this->rotate(vector3df(rot, 0.0f, 0.0f) );
}

void				CSceneObject::roll(f32 rot)
{
    this->rotate(vector3df(0.0f, 0.0f, rot) );
}


 vector3df	CSceneObject::getTargetBearing(void) {
	return this->getTargetBearing(this->getTarget()->getPosition());
}
 vector3df	CSceneObject::getTargetBearing(vector3df target) {
	return(getYXangle(getRelativePosition(this->getPosition(), this->getRotation(), target)));
}
void		CSceneObject::setBearing(f32 x, f32 y) {
	matrix4 m1, m2;

	m1.setRotationDegrees(this->getRotation());
	m2.setRotationDegrees(vector3df(x, y, 0));

	this->setRotation( (m1 *= m2).getRotationDegrees() );
}
 vector3df	CSceneObject::getVelocityAtTarget(vector3df target, f32 speed) {
	matrix4 mat;

	vector3df vel = vector3df(0,0,speed);
	mat.setRotationDegrees(this->getRotationAtTarget(target));
	mat.transformVect(vel);
	return vel;
}
 vector3df	CSceneObject::getVelocityAtTarget(f32 speed) {
	matrix4 mat;
	
	if(!this->getTarget())
		return vector3df(0,0,0);

	vector3df vel = vector3df(0,0,speed);
	mat.setRotationDegrees(this->getRotationAtTarget(this->getTarget()->getPosition()));
	mat.transformVect(vel);
	return vel;
}
 void		CSceneObject::setVelocityAtTarget(vector3df target, f32 speed) {
	matrix4 mat;

	vector3df vel = vector3df(0,0,speed);
	mat.setRotationDegrees(this->getRotationAtTarget(target));
	mat.transformVect(vel);
	this->setVelocity(vel);
}
 void		CSceneObject::setVelocityAtTarget(f32 speed) {
	matrix4 mat;

	if(!this->getTarget())
		return;

	vector3df vel = vector3df(0,0,speed);
	mat.setRotationDegrees(this->getRotationAtTarget(this->getTarget()->getPosition()));
	mat.transformVect(vel);
	this->setVelocity(vel);
}
vector3df	CSceneObject::getRotationAtTarget(void) {
	if(!this->getTarget())
		return vector3df(0,0,0);
	vector3df diff = this->getTarget()->getPosition() - this->getPosition();
	return diff.getHorizontalAngle();
}
 vector3df	CSceneObject::getRotationAtTarget(vector3df target) {
	vector3df diff = target - this->getPosition();
	return diff.getHorizontalAngle();
}
void		CSceneObject::setRotationAtTarget(void) {
	if(!this->getTarget())
		return;
	this->setRotationAtTarget(this->getTarget()->getPosition());
}
 void		CSceneObject::setRotationAtTarget(vector3df target) {
	this->setRotation(this->getRotationAtTarget(target));
}




void				CSceneObject::setVelocity(vector3df value)		{ 
	if(this->mPhysxObject)
		this->mPhysxObject->setLinearVelocity(value);
}
vector3df			CSceneObject::getVelocity(void)				{ 
	vector3df ret;
	
	if(this->mPhysxObject)
		this->mPhysxObject->getLinearVelocity(ret);
	return ret;
}



void				CSceneObject::setRotation(vector3df value)		{ 
	if(this->mPhysxObject)
		this->mPhysxObject->setRotation(value);
	if(this->mSceneNode)
		this->mSceneNode->setRotation(value);
}
vector3df			CSceneObject::getRotation(void)				{ 
	vector3df ret;
	if(this->mPhysxObject)
		this->mPhysxObject->getRotation(ret);
	return ret;
}

f32				CSceneObject::getSpeed(void)				{ 
	vector3df vel;
	matrix4 m;

	vel = this->getVelocity();
	m.setRotationDegrees(this->getRotation());
	m.transformVect(vel);

	return vel.Z;
}
vector3df			CSceneObject::getPosition(void)				{ 
	vector3df d = vector3df(0,0,0);
	if(this->mPhysxObject) 
		this->mPhysxObject->getPosition(d); 
	return d;
} 
void				CSceneObject::setPosition(vector3df value)	{ 
	if(this->mPhysxObject)
		this->mPhysxObject->setPosition(value);
	if(this->mSceneNode)
		this->mSceneNode->setPosition(value);
}
 void		CSceneObject::setSceneNode(ISceneNode *node) {
	node->setID(this->getID());
	this->mSceneNode = node;
}
 s32		CSceneObject::getUniqueObjectID(void) {
	static s32 id_count = 0;
	return id_count++ ;
}

/* constructor/destructor */
CSceneObject::CSceneObject(void) {
	this->mAcceleration = 0.0f;
	this->mSceneNode = NULL;
	this->mPhysxObject = NULL;
	this->mPhysxMesh = NULL;
	this->mTarget = NULL;
	this->misDestroyed = false;
	this->mID = CSceneObject::getUniqueObjectID();
}

CSceneObject:: CSceneObject(IFileReadCallBack *file) {
	this->mAcceleration = 0.0f;
	this->mSceneNode = NULL;
	this->mPhysxObject = NULL;
	this->mPhysxMesh = NULL;
	this->mTarget = NULL;
	this->misDestroyed = false;
	this->mID = CSceneObject::getUniqueObjectID();

	this->deserialize(file);
}
CSceneObject::~CSceneObject() {
	OBJSTRMAP list;
	
	if(this->mPhysxMesh) {
		CGameManager::getInstance()->getPhysxManager()->removePhysxMesh(this->mPhysxMesh);
		delete this->mPhysxMesh;
	}
	if(this->mPhysxObject) {
		CGameManager::getInstance()->getPhysxManager()->removePhysxObject(this->mPhysxObject);
	}

	if(this->mSceneNode) {
		this->mSceneNode->remove();
	}
}